// Alvaro, 8/5/2011

//  hardwareIO.cpp

// COMMENT THE LINE BELOW TO WORK WITH REAL HARDWARE, and UNCOMMENT IT FOR TEST WITH ONLY THE ARDUINO CONNECTED TO THE COMPUTER, 
// AND USING THE PROCESSING PROGRAM "mirrorSimulator.pde"
//#define NO_HARDWARE


//#include "WProgram.h"
#include "hardwareIO.h"

// Control buttons on pins: 22, 23, 24, 25, 26, 27, 28 and 29, this is register PORTA (PA0 to PA7)
//int controlButtons[]={22, 23, 24, 25, 26, 27, 28, 29}; //this is register PORTA (PA0 to PA7)
//int ledButtons[]=    {37, 36, 33, 32, 35, 34, 31, 30}; // register PORTC (PC0 is 37, PC6 is 30)



HardwareIO IO; // preintantiation of cross-file global object IO

// --------------------------------------  (0) SETUP ALL IO (call this in the setup() function in main program)

void HardwareIO::init(void) {
      // for tests:
      // pinMode(30, OUTPUT); digitalWrite(30,HIGH); delay(200); digitalWrite(30, LOW); delay(200); digitalWrite(30,HIGH); delay(200); digitalWrite(30, LOW);   delay(200); digitalWrite(30,HIGH); 

	// Signal generation: 
	setupPWM(); //setup PWM timers for the reference signal and buzzer
  
          // Start with DAC CS pins to high (meaning not active). 
          // NOTE: IT IS THEREFORE IMPORANT TO CORRECTLY SET THE PORT VALUE when setting the CS to high or low, if we don't use a digitalWrite!!! 
          pinMode(CS_DAC_MIRRORS, OUTPUT);
          pinMode(CS_DAC_REDLASER, OUTPUT);
          digitalWrite(CS_DAC_MIRRORS, HIGH);
          digitalWrite(CS_DAC_REDLASER, HIGH);
 
	// Reference frequency: 
	//setRefFreq(186); // set to 85.5 kHz

	// LockIn: 
	setAnalogReadMode(EIGHT_BITS); // for lock-in readout mode (can be TEN or HEIGHT bits)

	// Green laser: 
	pinMode(greenLaserPin, OUTPUT);

  
        // just for "security" (we do this in the main program)
        setRedPower_mW(MAX_RedLaserPower);
        //Spi.writeOutR(2000);
        
        setGreenPower(LOW);
        
       // Buttons: 
       pinMode(pinSwitch, INPUT);
       digitalWrite(pinSwitch,HIGH);// to activate pullup resistors (i.e., when switch is off, the pin reads HIGH). 
      /*
	for (int i=0; i<8; i++) {
		pinMode(controlButtons[i], INPUT); //(buttons as inputs)
		digitalWrite(controlButtons[i], HIGH); // (set pull up resistors)
		pinMode(ledButtons[i], OUTPUT); // button leds as outputs
	}
      */
      
       // ANALOG PINS: 
       analogReference(DEFAULT); // this means 5V on the arduino. INTERNAL2V56: a built-in 2.56V reference (Arduino Mega only) 
       
      // ATTN: normally there is no need to set the inputs as INPUTS, unless there were configured otherwise 
      //("The analogRead command will not work correctly if a pin has been previously set to an output,"
       // For security, we do it here (actually, wihout doing this, I had a problem!). BEWARE: we need to use A0, A1, A2, not 0, 1 and 2..
     
      // Lockin pin:
      // NOTE: PROBLEM!!! A0 seems NOT TO WORK!!!
      pinMode(A0, INPUT); // corresponds to analog input pin 0 (LOCKIN_PIN)
      digitalWrite(A0, LOW); // reset pullup resistors
      // I am using then analog pin 8 instead
      
      // potentiometers:
      pinMode(A1, INPUT); // corresponds to analog input pin 1 (potA_pin)
      digitalWrite(A1, LOW); // reset pullup resistors
      pinMode(A2, INPUT); // corresponds to analog input pin 2 (potB_pin)
      digitalWrite(A2, LOW); // reset pullup resistors

      
	// Buzzer: 
	//setBuzzerFreq(500); // buzzer frequency (OCR4=500 makes about 4kHz)
	//setBuzzer(LOW); // stop buzzer

	// mirror degree-to-AD units conversion factors: 
	AD_to_Deg_MIRROR_X=1.0*(MAX_DEG_MIRROR_X-MIN_DEG_MIRROR_X)/(MAX_AD_MIRRORS-MIN_AD_MIRRORS);
	AD_to_Deg_MIRROR_Y=1.0*(MAX_DEG_MIRROR_Y-MIN_DEG_MIRROR_Y)/(MAX_AD_MIRRORS-MIN_AD_MIRRORS);

// SET THE interruption for button readout (on interruptButtonPin):
//pinMode(interruptButtonPin, INPUT);
// THIS IS A HACK: one cannot use attachInterrupt on a class function, that's why I declare a global function that calls the class method in the setup()
//  attachInterrupt(INTERRUPT_READ_BUTTONS_PIN, readButtonGlobal, RISING); 

//processButton=false;
 
 
// LCD DISPLAY on serial port 3: 
Serial3.begin(9600); 
// Control backlight power
Serial3.write(128); // back light command will follow:
Serial3.write(130); // power (from 128 to 157)

 
}


void HardwareIO::clearLCD() {
	//clear the screen:
	Serial3.write(254); // LCD command will follow:
	Serial3.write(1); // command meaning clear screen
}

void clearLine(int line) {
	Serial3.print(0xFE, BYTE);   //command flag for cursor setting
	if (line==1)  Serial3.print(128, BYTE); else  Serial3.print(192, BYTE);  //set position on start line 1 or 2
	Serial3.print("                "); 
}

void HardwareIO::displayLineLCD(char *string, int line) { 
	// Set cursor on the requested line: 
	Serial3.print(0xFE, BYTE);   //command flag for cursor setting
	if (line==1)  Serial3.print(128, BYTE); else  Serial3.print(192, BYTE);  //set position on start line 1 or 2

	// Print the data: 
	Serial3.print(string); // ATTN: string must be well terminated ('/0')
}

// overloaded method that takes a float:
void HardwareIO::displayLineLCD(float num, int line) { // just take what is in twoLines[2][17] and display: 
	// Set cursor on the requested line: 
	Serial3.print(0xFE, BYTE);   //command flag for cursor setting
	if (line==1)  Serial3.print(128, BYTE); else  Serial3.print(192, BYTE);  //set position on start line 1 or 2

	// Print the data: 
	Serial3.print(num, 3); // if num is a float, this prints in ASCII, base 10, and the second parameter specifies the number of decimal places
}

// overloaded method that takes a float:
void HardwareIO::displayLineLCD(byte num, int line) { // just take what is in twoLines[2][17] and display: 
	// Set cursor on the requested line: 
	Serial3.print(0xFE, BYTE);   //command flag for cursor setting
	if (line==1)  Serial3.print(128, BYTE); else  Serial3.print(192, BYTE);  //set position on start line 1 or 2

	// Print the data: 
	Serial3.print(num, BIN); // if num is a float, this prints in ASCII, base 10, and the second parameter specifies the number of decimal places
}

/*
void HardwareIO::readButtons() {
	buttonByte=BUTTON_BYTE_PORT;
	processButton=true;
}
*/

void HardwareIO::setAnalogReadMode(int mode) {
	// to do: check for consistency modes
	analogReadMode=mode;
}

// =======================================  (1) ADC for PHOTODETECTOR SENSING (after LOCK IN AMP):

#ifdef NO_HARDWARE
// THIS SEND DATA TO PROCESSING:
// readout directly in card units: 
int HardwareIO::LockInRead_AD() { // mode is selected from the method variable analogReadMode
	int readout=-1;
	Serial.print('R'); 
        while (!Serial.available());
        readout=Serial.read();
      switch(analogReadMode) {
	case EIGHT_BITS: // use fast analog read
                //readout=FastAnalogRead(LOCKIN_PIN);
		break;
	case TEN_BITS:
		readout*=4; //analogRead(LOCKIN_PIN);
		break;
	}
	return (readout);
}
// readout as a voltage: 
float HardwareIO::LockInRead_Volts() {
	float readout=-1;
         Serial.print('R'); 
        while (!Serial.available());
        readout=Serial.read();
	switch(analogReadMode) {
	case EIGHT_BITS: // use fast analog read and convert 0-255 to 0-5V
		readout=1.0*readout*5.0/255;
		break;
	case TEN_BITS:// use Arduino analogRead() function, and convert 0-1024 to 0-5V
		readout=4.0*readout*5.0/1024;
		break;
	}
	return (readout);
}
#else
// readout directly in card units: 
int HardwareIO::LockInRead_AD() { // mode is selected from the method variable analogReadMode
	int readout=-1;
	switch(analogReadMode) { // DOING THIS TEST MAY SLOWDOWN THINGS!!!!
	case EIGHT_BITS: // use fast analog read:
		readout=FastAnalogReadPin8();
		break;
	case TEN_BITS:
		readout=analogRead(LOCKIN_PIN);
		break;
	}
	return (readout);
}
// readout as a voltage: 
float HardwareIO::LockInRead_Volts() {
	float readout=-1;
	switch(analogReadMode) {
	case EIGHT_BITS: // use fast analog read and convert 0-255 to 0-5V
		readout=1.0*FastAnalogReadPin8()*5.0/255;
		break;
	case TEN_BITS:// use Arduino analogRead() function, and convert 0-1024 to 0-5V
		readout=1.0*analogRead(LOCKIN_PIN)*5.0/1024;
		break;
	}
	return (readout);
}
#endif

float HardwareIO::LockInAD_to_Volts(int readAD) {
 float volts=-1;
	switch(analogReadMode) {
	case EIGHT_BITS: // use fast analog read and convert 0-255 to 0-5V
		volts=1.0*readAD*5.0/255;
		break;
	case TEN_BITS:// use Arduino analogRead() function, and convert 0-1024 to 0-5V
		volts=1.0*readAD*5.0/1024;
		break;
	}
	return (volts);
}


//modification of ADC prescaler for fast reading of analog Pin 0
//read only ADCH -> High 8-bits
//modifications include:
//  - "ADLAR" left adjusted (ADCH = High 8-bits result)
//  - "ADPS" Clk / 32 (faster clock division; fast sampling but lose occuracy which is OK because we read only ADCH)
byte HardwareIO::FastAnalogReadPin0(){
	//ADC prescaler:
	ADMUX = B01100000;
	ADCSRB = B00000000;
	ADCSRA = B10010101;

	// start the conversion
	ADCSRA = B11010101;
	// ADSC is cleared when the conversion finishes
	while (bit_is_set(ADCSRA, ADSC));

	return ADCH;
}

//modification of ADC prescaler for fast reading of analog Pin 0
//read only ADCH -> High 8-bits
//modifications include:
//  - "ADLAR" left adjusted (ADCH = High 8-bits result)
//  - "ADPS" Clk / 32 (faster clock division; fast sampling but lose occuracy which is OK because we read only ADCH)
byte HardwareIO::FastAnalogReadPin8(){
	//parameters for pin 8 (see page 291 of ATmega doc) and analogRead in wiring_analog.c library
       
	ADMUX = B01100000; // set the analog reference (high two bits of ADMUX) and select the channel (low 4 bits).  
      // this also sets ADLAR (left-adjust result) to 1 (NOT the default for analogRead), because we want to read only one byte

	ADCSRB = B00001000; // the MUX5 bit of ADCSRB selects whether we're reading from channels 0 to 7 (MUX5 low) or 8 to 15 (MUX5 high).
	ADCSRA = B10010101;

	// start the conversion
	ADCSRA = B11010101;
	// ADSC is cleared when the conversion finishes
	while (bit_is_set(ADCSRA, ADSC));

	return ADCH;
}

//(2) =======================================  DAC (control mirrors and red laser):

// LOW LEVEL: 
//write on the first DAC, output A (mirror X)
void HardwareIO::writeOutX(int value){
  //Set LOW on the Chip Select of the mirrors DAC
  PORTC &= B11111110; // ENABLE FIRST DAC (mirrors) by putting pin 37 to LOW (this is, PORTC bit PC0) 
  
  SPDR = (byte)(value >> 8) | B01010000 & B01011111;
  while (!(SPSR & (1<<SPIF))) ;
  
  SPDR = (byte)value;
  while (!(SPSR & (1<<SPIF))) ;
  
  //Set HIGH on the Chip Select of the mirrors DAC
  PORTC |= B00000001;
}

//write on the first DAC, output B (mirror Y)
void HardwareIO::writeOutY(int value){
  //Set LOW on the Chip Select of the mirrors DAC
  PORTC &= B11111110;
  
  SPDR = (byte)(value >> 8) | B11010000 & B11011111; 
  while (!(SPSR & (1<<SPIF))) ;
  
  SPDR = (byte)value;
  while (!(SPSR & (1<<SPIF))) ;
  
  //Set HIGH on the Chip Select of the mirrors DAC
  PORTC |= B00000001;
}

//write on the second DAC, output A (red Laser)
void HardwareIO::writeOutR(int value){
  //Set LOW on the Chip Select of the red laser DAC:
  PORTC &= B11111101;// ENABLE SECOND DAC (laser) by putting pin 36 to LOW (this is, PORTC bit PC1) 

 //creates 2 bytes containing the Highest part and the Lowest part of the 16-bits message
  byte MSBs = (byte)(value >> 8) | B01110000 & B01111111; //(byte)(value>>8) & B00001111 | B01110000;
  byte LSBs = (byte)(value);
  //send the Highest part first, then the lowest part
  SPI.transfer(MSBs);
  SPI.transfer(LSBs);

/* old code (HANGS!!!!) 
  SPDR = (byte)(value >> 8) | B01110000 & B01111111;
  while (!(SPSR & (1<<SPIF))) ;
  
  SPDR = (byte)value;
  while (!(SPSR & (1<<SPIF))) ;
 */
  
  //Set HIGH on the Chip Select of the laser DAC
  PORTC |= B00000010;
}


// To be used like this:
//      IO.setMirrorX_AD(int(1.0*Az + _saccadePosArray[2 * i]*radius));
//	IO.setMirrorY_AD(int(1.0*El + _saccadePosArray[2 * i + 1]*radius));
// -----------------MIRRORS:
void HardwareIO::setMirrorX_Deg(float AzDeg) {
	// AD value for X mirror =  (AzDeg-MIN_DEG_MIRROR_X)/(MAX_DEG_MIRROR_X-MIN_DEG_MIRROR_X)*(MAX_AD_MIRRORS-MIN_AD_MIRRORS)
	// AD_to_Deg_MIRROR_X=1.0*(MAX_DEG_MIRROR_X-MIN_DEG_MIRROR_X)/(MAX_AD_MIRRORS-MIN_AD_MIRRORS);
	writeOutX(int(1.0*(AzDeg-MIN_DEG_MIRROR_X)/AD_to_Deg_MIRROR_X));
}

void HardwareIO::setMirrorY_Deg(float ElDeg) {
	writeOutY(int(1.0*(ElDeg-MIN_DEG_MIRROR_Y)/AD_to_Deg_MIRROR_Y));
}

// DIRECT conversion from AD values:
void HardwareIO::setMirrorX_AD(int _AzAD) {
	writeOutX(_AzAD);
}

void HardwareIO::setMirrorY_AD(int _ElAD) {
	writeOutY(_ElAD);
}

void HardwareIO::setMirrorsXY_AD(int _xAD, int _yAD) {
	writeOutX(_xAD);
	writeOutY(_yAD);
}

void HardwareIO::setMirrorsCenter() {
	writeOutX(CENTER_AD_MIRROR_X);
	writeOutY(CENTER_AD_MIRROR_Y);
}

// get azimut and elevation angles in degrees from AD values: 
void HardwareIO::getAnglesFromAD(float &AzDeg, float &ElDeg, int AzAD, int ElAD) {
	AzDeg=AzAD*AD_to_Deg_MIRROR_X+MIN_DEG_MIRROR_X;
	ElDeg=ElAD*AD_to_Deg_MIRROR_Y+MIN_DEG_MIRROR_Y;
}

// =======================================  LASER RED:
void HardwareIO::setRedPower_mW(float power_mW) { // this is the power IN MILLIWATS
if (power_mW>0) {
        int auxPowerAD=int(power_mW*mW_to_AD_RED);
	if (auxPowerAD<=MAX_AD_RED) writeOutR(auxPowerAD); 
	else writeOutR(MAX_AD_RED); 
        // don't forget to set it up:
       // OCR1B=ICR1/2; // this is for the new system
} else {
       // OCR1B=ICR1;
}
}

void HardwareIO::setRedPower_AD(int powerAD) { // this is the power in AD units
if (powerAD>0) {
	if (powerAD<=MAX_AD_RED) writeOutR(powerAD); 
	else writeOutR(MAX_AD_RED); 
        // don't forget to set it up:
       // OCR1B=ICR1/2;// this is for the new system
} else {
       // OCR1B=ICR1;
}
}


// (3)  ======================================= GREEN LASER:
// nothing to do (just use digitalWrite), but let's wrapp the function so it looks alike the red laser:
void HardwareIO::setGreenPower(boolean state) { // HIGH or LOW
	digitalWrite(greenLaserPin, state);
}

// (4) ======================================= SET PRESCALERS (for lockin reference generation):

void HardwareIO::setBuzzerFreq(int param_icr4) {
	ICR4=(byte)param_icr4;
        OCR4A = (byte)(param_icr4/2); 
	//OCR4B = (byte)(param_icr4/2); 
	//OCR4C =(byte)(param_icr4/2); 
}

void HardwareIO::setBuzzer(boolean state) {
	if (state) OCR4A=(byte)ICR4/2; 
	else OCR4A=0;
}

void HardwareIO::buzzerBip(int timebip, int times) {
	for (int i=0; i< times; i++) {
		setBuzzer(HIGH); 
                delay(timebip); 
                setBuzzer(LOW); 
	} 
}



// Read potentiometers (rem: values will be normalized):
float HardwareIO::readPotA() {
	//We will read using the standard analogRead function, so we have 0-5V mapped to 0-1024:
	valuePotA=1.0*analogRead( potA_pin )/1023; //normalized
	return(valuePotA);
}

float HardwareIO::readPotB() {
	//We will read using the standard analogRead function, so we have 0-5V mapped to 0-1024:
	valuePotB=1.0*analogRead( potB_pin )/1023;
	return(valuePotB);
}

void HardwareIO::setupPWM(){
  
  pinMode (testPin_OC1A, OUTPUT);
  pinMode (testPin_OC1B, OUTPUT);
  pinMode (testPin_OC1C, OUTPUT);

  //(1) Phase and Frequency Correct PWM Mode for Timer/Counter 1, with TOP=ICR1: WGM1[3:0] = 8 
  // TCCR1A bits:	        COM1A1 COM1A0 COM1B1 COM1B0 COM1C1 COM1C0 WGM11 WGM10
  // set bits:	                                                          0     0
  // TCCR1B bits:	        ICNC1  ICES1  -      WGM13  WGM12  CS12   CS11  CS10 
  // set bits:                                       1      0

  // (1.b) Or Fast PWM with TOP=ICR1: WGM1[3:0] = B1110 (=14) 
  // TCCR1A bits:	        COM1A1 COM1A0 COM1B1 COM1B0 COM1C1 COM1C0 WGM11 WGM10
  // set bits:	                                                          1     0
  // TCCR1B bits:	        ICNC1  ICES1  -      WGM13  WGM12  CS12   CS11  CS10 
  // set bits:                                       1      1

  // (2) When in Phase and Freq. correct or on fast PWM, non inverted PWM output on OC1A when COM1A[1:0]=B11, and on OC1B when COM1B[1:0]=B11
  // TCCR1A bits:	        COM1A1 COM1A0 COM1B1 COM1B0 COM1C1 COM1C0 WGM11 WGM10
  // set bits:	                1      1      1      1       1    1       X     X

  // (3)  Use internal clock without prescaling (N=1) by setting: CS1[2:0] = B001 (TCCR1B[2:0]=B001):
  // TCCR1B bits:	        ICNC1  ICES1  -      WGM13  WGM12  CS12   CS11  CS10 
  // set bits:                  X      X      X      X      X      0      0     1

  // Conclusion: overriding the default values of the control registers TCCR1A and TCCR1B (this will affect pins 11, 12 and 13), we would do: 
  // For phase and frequency correct PWM:
  //TCCR1A = B11111100;
  //TCCR1B = B00010001;
  // And for fast PWM:
  TCCR1A = B11111110;
  TCCR1B = B00011001;
  

  // TOP VALUE for the TCNT1 counter is ICR1 (and gives the resolution):
  ICR1 = 64 ; // 10 bit resolution (perhaps not enough for smoothly positioning and generating the laser saccade, but let's try)
  // This means:
  // (a) for phase and frequency correct: Fpwm= 16e6/(2*1024*1) = 7.8125 kHz, or Tpwm= 1/Fpwm = 128us
  // (b) for fast PWM: double of this (15.625kHz, period of 64 us).

  // The PWM duty cycle on OC1A pin (pin 11) is given by the value of the register OCR1A:
  OCR1A = 1; // vary this value between 0 and ICR1=1024 for 10-bit precision
  OCR1B = 36;// best value, calibrate by hand (will generate the same square signal but shifted by (OCR1B-OCR1A)/16MHz
}

/*  NOTE: in case of variable frequency: 
void HardwareIO::setRefFreq(int param_icr1) {

	ICR1=param_icr1; 
	OCR1A = (byte)(param_icr1/2); 
	OCR1B = (byte)(param_icr1/2); 
	OCR1C =(byte)(param_icr1/2); 

	// current reference frequency: 
	refFreq=(float)16000.0/param_icr1;
}

void HardwareIO::incRefFreq(int increment) {
	setRefFreq(ICR1+increment);
}
void HardwareIO::decRefFreq(int decrement) {
	if ((ICR1-decrement)>=0) setRefFreq(ICR1-decrement);
}

void HardwareIO::initPWM(){

	pinMode (testPin_OC1A, OUTPUT);
	pinMode (testPin_OC1B, OUTPUT);
	pinMode (testPin_OC1C, OUTPUT);
	pinMode (buzzer_pin, OUTPUT); // attention: pin 6, 7, 8 sont associes. 

	//(1) Phase and Frequency Correct PWM Mode for Timer/Counter 1, with TOP=ICR1: WGM1[3:0] = 8
	// TCCR1A bits:	        COM1A1 COM1A0 COM1B1 COM1B0 COM1C1 COM1C0 WGM11 WGM10
	// set bits:	                                                          0     0
	// TCCR1B bits:	        ICNC1  ICES1  -      WGM13  WGM12  CS12   CS11  CS10
	// set bits:                                       1      0

	// (1.b) Or Fast PWM with TOP=ICR1: WGM1[3:0] = B1110 (=14)
	// TCCR1A bits:	        COM1A1 COM1A0 COM1B1 COM1B0 COM1C1 COM1C0 WGM11 WGM10
	// set bits:	                                                          1     0
	// TCCR1B bits:	        ICNC1  ICES1  -      WGM13  WGM12  CS12   CS11  CS10
	// set bits:                                       1      1

	// (2) When in Phase and Freq. correct or on fast PWM, non inverted PWM output on OC1A when COM1A[1:0]=B11, and on OC1B when COM1B[1:0]=B11
	// TCCR1A bits:	        COM1A1 COM1A0 COM1B1 COM1B0 COM1C1 COM1C0 WGM11 WGM10
	// set bits:	                1      1      1      1       1    1       X     X

	// (3)  Use internal clock without prescaling (N=1) by setting: CS1[2:0] = B001 (TCCR1B[2:0]=B001):
	// TCCR1B bits:	        ICNC1  ICES1  -      WGM13  WGM12  CS12   CS11  CS10
	// set bits:                  X      X      X      X      X      0      0     1

	// Conclusion: overriding the default values of the control registers TCCR1A and TCCR1B (this will affect pins 11, 12 and 13), we would do:
	// For phase and frequency correct PWM:

	// For the reference signal: 
	//TCCR1A = B11111100; 
	//TCCR1B = B00010001;
	// And for fast PWM:
	TCCR1A = B11111110;
	TCCR1B = B00011001; // prescaler of 1

	// for the buzzer:
	TCCR4A = B11111110; 
	TCCR4B = B00011010; // prescaler of 8

	// The actual frequency (for the reference) is set by setting ICR1, OCR1A/B/C (function setRefFreq)

}
*/



